% x is a weighted donut on a vertical pole.  Simulate the movement
% of the donut given the target and the random commands.
% Force must depend on whether you wish to hold it still, or move it.
% Control system should allow you to specify the speed of movement and
% the desired position; the current position/vel is known, so these 4
% are enough to determine the control signal.  
% The idea is to create a map of these 4 variables and associate the
% appropriate command signal that will get you there at a certain time?

% if you just increase the input to all cells for higher speed, you get
% higher firing rate => 

xmax = 12; % inches.


x = 0;  % at bottom
xd = 0; %not moving.

a = exp(-1/20);
m = 1.0;
f = 0.0;

tau_sec = .01;


command_countdown = -1;

sim_seconds = 1000;

x1=zeros(1,ceil(sim_seconds/tau_sec));
f1=x1;
xd1=x1;
xdd1=x1;


g= 1.0;

for t = 1:sim_seconds/tau_sec
    
    if( command_countdown < 0)
        command_countdown = .25/tau_sec;
        xd_command = rand*4-2;
    end
        

    % low pass filter muscle.
    f = a*f + (1-a)*(xd_command - g) ;
    
    xdd = f/m;
    
    old_xd = xd;
    xd = xd + tau_sec*xdd;
    
    old_x = x;
    x = x + tau_sec*xd;
   
    % clip
    if x > xmax
        xdd = 0-old_xd;
        xd=xmax-old_x;
        x=xmax;
    end
    if x < 0
        xdd = 0-old_xd;
        xd=0-old_x;
        x=0;
    end
        
    
    % save history
    x1(t)=x;
    f1(t)=f;
    xd1(t)=xd;
    xdd1(t)=xdd;
    
    command_countdown = command_countdown - 1;
    
end

    
subplot(2,1,1)
plot(x1);
subplot(2,1,2);
plot([f1',xd1',xdd1']);
legend('f','xd','xdd');

%plot3(f1,xd1,xdd1,'.');
